#include "My_INC/APP/device_handlers.h"
#include "My_INC/APP/APP_ACLoad_Sys.h"
#include "My_INC/APP/error_codes.h"
#include "My_INC/PHY/PHY_Print.h"

// 命令处理器数组及其大小
#define MAX_CMD_HANDLERS 32 // 最大支持32个命令处理器
static cmd_handler_entry_t cmd_handlers[MAX_CMD_HANDLERS] = {0};
static int                 cmd_handler_count = 0;

uint8_t MainMode = 0; // 主模式
uint8_t SubMode = 0;  // 子模式

// 查找命令处理器
cmd_handler_entry_t *find_cmd_handler(uint8_t cmd)
{
    for (int i = 0; i < cmd_handler_count; i++)
    {
        if (cmd_handlers[i].cmd == cmd)
        {
            return &cmd_handlers[i];
        }
    }
    return NULL;
}

// 注册命令处理器
err_status_t register_cmd_handler(const cmd_handler_entry_t *handler)
{
    if (handler == NULL || handler->handler == NULL)
    {
        return ERR_INVALID_ARG;
    }

    // 检查是否已存在
    if (find_cmd_handler(handler->cmd) != NULL)
    {
        return ERR_INVALID_STATE;
    }

    // 检查是否超出最大数量
    if (cmd_handler_count >= MAX_CMD_HANDLERS)
    {
        return ERR_NO_MEM;
    }

    // 添加到数组
    cmd_handlers[cmd_handler_count] = *handler;
    cmd_handler_count++;

    Uart1_Printf("注册命令处理器: 0x%02x (%s)\r\n", handler->cmd, handler->description);
    return ERR_OK;
}

// 注销命令处理器
err_status_t unregister_cmd_handler(uint8_t cmd)
{
    for (int i = 0; i < cmd_handler_count; i++)
    {
        if (cmd_handlers[i].cmd == cmd)
        {
            // 移动后面的元素
            for (int j = i; j < cmd_handler_count - 1; j++)
            {
                cmd_handlers[j] = cmd_handlers[j + 1];
            }
            cmd_handler_count--;
            Uart1_Printf("注销命令处理器: 0x%02x\r\n", cmd);
            return ERR_OK;
        }
    }
    return ERR_NOT_FOUND;
}

// 处理目标电压电流信号
static cmd_result_t handle_voltage_current(const uint16_t *data, uint8_t length)
{
    uint16_t Target_voltage = data[0];
    uint16_t Target_current = data[1];
    Uart1_Printf("目标电压值: %d, 目标电流值: %d\r\n", Target_voltage, Target_current);

    // TODO: 在这里添加目标电压电流处理逻辑
    // g_acLoadSystem.controller[1].param.il_ref = Target_current;
    // g_acLoadSystem.controller[1].param.vo_ref = Target_voltage;
    return CMD_SUCCESS;
}

// 处理开关机信号
static cmd_result_t handle_switch(const uint16_t *data, uint8_t length)
{
    uint16_t switch_state = data[0];
    Uart1_Printf("开关状态: %d\r\n", switch_state);

    // TODO: 在这里添加开关机处理逻辑
    if (switch_state == 1)
    {
        g_acLoadSystem.controller[0].loop_sm.start_flag = Activated;
        g_acLoadSystem.controller[0].loop_sm.stop_flag = Nonactivated;
    }
    else if (switch_state == 0)
    {
        g_acLoadSystem.controller[0].loop_sm.stop_flag = Activated;
        g_acLoadSystem.controller[0].loop_sm.start_flag = Nonactivated;
    }
    return CMD_SUCCESS;
}

// 处理模式切换
static cmd_result_t handle_mode_switch(const uint16_t *data, uint8_t length)
{
    uint16_t mode = data[0];
    Uart1_Printf("切换模式: %d\r\n", mode);

    // TODO: 在这里添加模式切换处理逻辑
    if (mode == 0) // 源模式选择
    {
        g_acLoadSystem.wock_mode = WOCK_MODE_SOURCE;
        g_acLoadSystem.device_mode = DEVICE_MODE_VOLTAGE;
    }
    else if (mode == 1) // 载模式选择
    {
        g_acLoadSystem.wock_mode = WOCK_MODE_LOAD;
    }
    return CMD_SUCCESS;
}

// 处理PS源模式设置参数
static cmd_result_t handle_ps_setting(const uint16_t *data, uint8_t length)
{
    // 接收的数据是真实数据的十倍，需要除以10
    float Ref_voltage = (float)data[0] * 0.1f;   // 设置参数1
    float Limit_current = (float)data[1] * 0.1f; // 设置参数2
    Uart1_Printf("PS源模式设置参数: 参考电压: %f, 限流: %f\r\n", Ref_voltage, Limit_current);

    // TODO: 在这里添加PS源模式设置参数处理逻辑
    g_acLoadSystem.controller[1].monitor.il_over_current_threshold = Limit_current * 1.414213562373095f;
    g_acLoadSystem.controller[1].param.vo_ref = Ref_voltage;

    g_acLoadSystem.controller[1].param.il_ref = 0.0f;
    g_acLoadSystem.controller[1].param.Phaseshift_angle = 0.0f;
    g_acLoadSystem.controller[1].param.pe_ref = 0.0f;

    return CMD_SUCCESS;
}

// 处理EL载模式子模式切换
static cmd_result_t handle_el_mode_switch(const uint16_t *data, uint8_t length)
{
    // 0-CC 1-CV 2-CP 3-CR
    uint16_t el_mode_switch_value = data[0]; // 子模式当前值
    Uart1_Printf("EL载模式子模式当前值: %d\r\n", el_mode_switch_value);
    SubMode = el_mode_switch_value; // 保存子模式
    // TODO: 在这里添加EL载模式子模式切换处理逻辑

    switch (el_mode_switch_value)
    {
    case 0:
        /* code */
        g_acLoadSystem.device_mode = DEVICE_MODE_CURRENT;
        break;
    case 1:
        /* code */
        g_acLoadSystem.device_mode = DEVICE_MODE_VOLTAGE;
        break;
    case 2:
        /* code */
        g_acLoadSystem.device_mode = DEVICE_MODE_POWER;
        break;
    case 3:
        /* code */
        g_acLoadSystem.device_mode = DEVICE_MODE_IMPEDANCE;
        break;
    default:
        break;
    }

    return CMD_SUCCESS;
}

// 处理EL载模式设置参数
static cmd_result_t handle_el_setting(const uint16_t *data, uint8_t length)
{
    uint16_t el_setting_1 = data[0]; // 设置参数1
    uint16_t el_setting_2 = data[1]; // 设置参数2
    Uart1_Printf("EL载模式设置参数: %d,%d\r\n", el_setting_1, el_setting_2);

    switch (SubMode)
    {
    case 0:                                                                  // CC
    	int16_t CC_I = (float)(el_setting_1 - 65535) * 0.1f;                                    // 电流
    	int16_t CC_Angle = (float)(el_setting_2 - 65535) * 0.00174532925199432957692369076849f; // 相位角

        if(el_setting_1 > 30000)
        {
        	CC_I = (el_setting_1 - 0x10000) ;
        }
        else
        {
        	CC_I = el_setting_1;
        }
        if(el_setting_2 > 30000)
        {
        	CC_Angle = (el_setting_2 - 0x10000) ;
        }
        else
        {
        	CC_Angle = el_setting_2;
        }
//        Uart1_Printf("CC模式设置参数: 电流: %d, 相位角: %d\r\n", CC_I, CC_Angle);
        // todo: 处理CC模式设置参数
        g_acLoadSystem.controller[1].param.il_ref = (float)(CC_I) * 0.1f;
        g_acLoadSystem.controller[1].param.Phaseshift_angle = (float)(CC_Angle) * 0.00174532925f;
        g_acLoadSystem.controller[1].param.vo_ref = 0.0f;
        g_acLoadSystem.controller[1].param.pe_ref = 0.0f;
        break;
    case 1:                           // CV
        uint16_t CV_V = el_setting_1 * 0.1f ; // 电压
        uint16_t CV_I = el_setting_2 * 0.1f; // 电流
        Uart1_Printf("CV模式设置参数: 电压: %d, 电流: %d\r\n", CV_V, CV_I);
        // todo: 处理CV模式设置参数
        // 暂时不写
        break;
    case 2:                           // CP
        int16_t  CP_P = 0; // 功率
        // todo: 处理CP模式设置参数
        if(el_setting_1 > 30000)
        {
        	CP_P = (el_setting_1 - 0x10000) ;
        }
        else
        {
        	CP_P = el_setting_1;
        }
        g_acLoadSystem.controller[1].param.il_ref = 0.0f;
        g_acLoadSystem.controller[1].param.vo_ref = 0.0f;
        g_acLoadSystem.controller[1].param.Phaseshift_angle = 0.0f;
//        Uart1_Printf("CP模式设置参数: 功率: %d\r\n", CP_P);
        break;
    case 3: // CR
        break;
    default:
        break;
    }
    g_acLoadSystem.controller[1].monitor.il_over_current_threshold =  8.4852813742385702928101323452582f;
    return CMD_SUCCESS;
}

// 注册所有命令处理器
void register_device_handlers(void)
{
    // 注册电压电流信号处理器
    cmd_handler_entry_t voltage_current_handler = {.cmd = CMD_VOLTAGE_CURRENT_SIGNAL,
                                                   .min_length = 2,
                                                   .handler = handle_voltage_current,
                                                   .description = "电压电流信号"};
    register_cmd_handler(&voltage_current_handler);

    // 注册开关机信号处理器
    cmd_handler_entry_t switch_handler = {
        .cmd = CMD_SWITCH_SIGNAL, .min_length = 1, .handler = handle_switch, .description = "开关机信号"};
    register_cmd_handler(&switch_handler);

    // 注册模式切换处理器
    cmd_handler_entry_t mode_handler = {
        .cmd = CMD_MODE_SWITCH, .min_length = 1, .handler = handle_mode_switch, .description = "模式切换"};
    register_cmd_handler(&mode_handler);

    // 注册PS源模式设置参数处理器
    cmd_handler_entry_t ps_setting_handler = {
        .cmd = CMD_PS_SETTING, .min_length = 2, .handler = handle_ps_setting, .description = "PS源模式设置参数"};
    register_cmd_handler(&ps_setting_handler);

    // 注册EL载模式设置参数处理器
    cmd_handler_entry_t el_setting_handler = {
        .cmd = CMD_EL_SETTING, .min_length = 2, .handler = handle_el_setting, .description = "EL载模式设置参数"};
    register_cmd_handler(&el_setting_handler);

    // 注册EL载模式子模式切换处理器
    cmd_handler_entry_t el_mode_switch_handler = {.cmd = CMD_EL_MODE_SWITCH,
                                                  .min_length = 1,
                                                  .handler = handle_el_mode_switch,
                                                  .description = "EL载模式子模式切换"};
    register_cmd_handler(&el_mode_switch_handler);
}
